#include <stdio.h>
#include "sb3500_regs.h"
#include "sb3500_clock.h"

typedef struct {
    unsigned long   pll_clk;    
    unsigned long   arm_clk;
    unsigned long   axi_clk;
    unsigned long   ahb_clk;
    unsigned long   apb_clk;
    unsigned long   dmc_clk;
    unsigned long   smc_clk;
    unsigned long   dsp_clk;
    unsigned long   hsn_clk;
} clock_cfg;

static clock_cfg clock_data;

void clock_init(void)
{
    unsigned long val=0, Fin;
    unsigned char OD, NF, NR;
    unsigned long divide;

    val = readl(SOC_DPMU_REG(SBDPMU_PLL_CTRL));

    OD = val & 0x3; /* bit 0:1 */
    NF = (val >> 2) & 0x7f; /* bit 2:8 */
    NR = (val >> 9) & 0x1f; /* bit 9:13 */

    //OD = 0;
    //NF = 54;
    //NR = 2;

    //printf("OD = %d\n", OD);
    //printf("NF = %d\n", NF);
    //printf("NR = %d\n", NR);

    Fin = OSC_XTAL_FREQ / 1000;
    divide = (NR + 1) * (1 << OD);
    clock_data.pll_clk = Fin * (2 * (NF + 1)) / divide;
    clock_data.pll_clk *= 1000;

    val = readl(SOC_DPMU_REG(SBDPMU_AMBA_CLK));

    clock_data.arm_clk = clock_data.pll_clk / ((val & 0x7) + 1); /* 0:2 */

    NF = (val >> 3) & 0x3;  /* 3:4 */
    clock_data.axi_clk = clock_data.arm_clk >> NF;    

    NF = (val >> 5) & 0x3; /* 5:6 */
    clock_data.ahb_clk = clock_data.arm_clk >> NF;    

    NF = (val >> 7) & 0x3; /* 7:8 */
    clock_data.apb_clk = clock_data.arm_clk >> NF;

    NF = (val >> 9) & 0x1; /* 9 */
    clock_data.dmc_clk = clock_data.axi_clk >> (NF + 1);    

    NF = (val >> 10) & 0x3; /* 10:11 */
    clock_data.smc_clk = clock_data.axi_clk >> NF;

    val = readl(SOC_DPMU_REG(SBDPMU_SH_CLK));

    if(val & (1 << 15))
    {
        clock_data.hsn_clk = clock_data.pll_clk;
    }
    else
    {
        NR = (val >> 12) & 0x3;
        clock_data.hsn_clk = clock_data.pll_clk >> (NR + 1);
    }

    if(val & (1 << 11))
    {
        clock_data.dsp_clk = clock_data.pll_clk;
    }
    else
    {
        NR = (val >> 8) & 0x3;
        clock_data.dsp_clk = clock_data.pll_clk >> (NR + 1);
    }
}

unsigned long clock_get_pll(void)
{
    return clock_data.pll_clk;
}

unsigned long clock_get_arm(void)
{
    return clock_data.arm_clk;
}

unsigned long clock_get_axi(void)
{
    return clock_data.axi_clk;
}

unsigned long clock_get_ahb(void)
{
    return clock_data.ahb_clk;
}

unsigned long clock_get_apb(void)
{
    return clock_data.apb_clk;
}

unsigned long clock_get_dmc(void)
{
    return clock_data.dmc_clk;
}

unsigned long clock_get_smc(void)
{
    return clock_data.smc_clk;
}

unsigned long clock_get_dsp(void)
{
    return clock_data.dsp_clk;
}

unsigned long clock_get_hsn(void)
{
    return clock_data.hsn_clk;
}

void Clock_printf(void)
{
	clock_init();

	printf("clock_get_pll = %d\n",  clock_get_pll());

	printf("clock_get_arm = %d\n",  clock_get_arm());

	printf("clock_get_axi = %d\n",  clock_get_axi());

	printf("clock_get_ahb = %d\n",  clock_get_ahb());

	printf("clock_get_apb = %d\n",  clock_get_apb());

	printf("clock_get_dmc = %d\n",  clock_get_dmc());

	printf("clock_get_smc = %d\n",  clock_get_smc());

	printf("clock_get_dsp = %d\n",  clock_get_dsp());

	printf("clock_get_hsn = %d\n\n\n",  clock_get_hsn());
}
